#include "senyun_driver/gmsl_camera_handler.h"

namespace v4l_cuda
{
    void GmslCameraHandler::start_camera()
    {
        open_device();
        init_device();
        init_cuda();
        start_capturing();
    }

    void GmslCameraHandler::close_camera()
    {
        stop_capturing();
        uninit_device();
        close_device();
        exit(EXIT_SUCCESS);
    }

    void GmslCameraHandler::process_image(void *p)
    {
        // printf("CUDA format conversion on frame %p\n", p);
        out_yuv_buffer = (unsigned char *)p;
        // cudaStreamAttachMemAsync(NULL, out_yuv_buffer, 0, cudaMemAttachHost);
        // 	cudaStreamSynchronize(NULL);
        // gpuConvertYUYVtoRGB((unsigned char *)p, out_buffer, width, height);
    }

    GmslCameraHandler::GmslCameraHandler(std::string device_name)
    {
        dev_name = device_name;
        // std::cout<<device_name<<" "<<dev_name<<std::endl;
    }

    int GmslCameraHandler::read_frame(void)
    {
        fd_set fds;
        struct timeval tv;
        int r;

        FD_ZERO(&fds);
        FD_SET(fd, &fds);

        /* Timeout. */
        tv.tv_sec = 2;
        tv.tv_usec = 0;

        r = select(fd + 1, &fds, NULL, NULL, &tv);

        if (-1 == r)
        {
            if (EINTR == errno)
                return 0;

            errno_exit("select");
        }

        if (0 == r)
        {
            fprintf(stderr, "select timeout\n");
            exit(EXIT_FAILURE);
        }
        struct v4l2_buffer buf;
        unsigned int i;

        CLEAR(buf);

        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_USERPTR;

        if (-1 == xioctl(fd, VIDIOC_DQBUF, &buf))
        {
            switch (errno)
            {
            case EAGAIN:
                return 0;

            case EIO:
                /* Could ignore EIO, see spec. */

                /* fall through */

            default:
                errno_exit("VIDIOC_DQBUF");
            }
        }

        for (i = 0; i < n_buffers; ++i)
            if (buf.m.userptr == (unsigned long)buffers[i].start && buf.length == buffers[i].length)
                break;

        assert(i < n_buffers);

        process_image((void *)buf.m.userptr);

        if (-1 == xioctl(fd, VIDIOC_QBUF, &buf))
            errno_exit("VIDIOC_QBUF");

        return 1;
    }

    void GmslCameraHandler::stop_capturing(void)
    {
        enum v4l2_buf_type type;

        type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

        if (-1 == xioctl(fd, VIDIOC_STREAMOFF, &type))
            errno_exit("VIDIOC_STREAMOFF");
    }

    void GmslCameraHandler::start_capturing(void)
    {
        unsigned int i;
        enum v4l2_buf_type type;

        for (i = 0; i < n_buffers; ++i)
        {
            struct v4l2_buffer buf;

            CLEAR(buf);

            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
            buf.memory = V4L2_MEMORY_USERPTR;
            buf.index = i;
            buf.m.userptr = (unsigned long)buffers[i].start;
            buf.length = buffers[i].length;

            if (-1 == xioctl(fd, VIDIOC_QBUF, &buf))
                errno_exit("VIDIOC_QBUF");
        }

        type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

        if (-1 == xioctl(fd, VIDIOC_STREAMON, &type))
            errno_exit("VIDIOC_STREAMON");
    }

    void GmslCameraHandler::uninit_device(void)
    {
        unsigned int i;
        for (i = 0; i < n_buffers; ++i)
        {
            cudaFree(buffers[i].start);
        }

        free(buffers);

        cudaFree(out_buffer);
    }

    void GmslCameraHandler::init_userp(unsigned int buffer_size)
    {
        struct v4l2_requestbuffers req;
        unsigned int page_size;

        page_size = getpagesize();
        buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);

        CLEAR(req);

        req.count = 4;
        req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        req.memory = V4L2_MEMORY_USERPTR;

        if (-1 == xioctl(fd, VIDIOC_REQBUFS, &req))
        {
            if (EINVAL == errno)
            {
                fprintf(stderr, "%s does not support "
                                "user pointer i/o\n",
                        dev_name.c_str());
                exit(EXIT_FAILURE);
            }
            else
            {
                errno_exit("VIDIOC_REQBUFS");
            }
        }

        buffers = (struct buffer *)calloc(4, sizeof(*buffers));

        if (!buffers)
        {
            fprintf(stderr, "Out of memory\n");
            exit(EXIT_FAILURE);
        }

        for (n_buffers = 0; n_buffers < 1; ++n_buffers)
        {
            buffers[n_buffers].length = buffer_size;
            cudaMallocManaged(&buffers[n_buffers].start, buffer_size, cudaMemAttachGlobal);

            if (!buffers[n_buffers].start)
            {
                fprintf(stderr, "Out of memory\n");
                exit(EXIT_FAILURE);
            }
        }
    }

    void GmslCameraHandler::init_device(void)
    {
        struct v4l2_capability cap;
        struct v4l2_cropcap cropcap;
        struct v4l2_crop crop;
        struct v4l2_format fmt;
        unsigned int min;

        if (-1 == xioctl(fd, VIDIOC_QUERYCAP, &cap))
        {
            if (EINVAL == errno)
            {
                fprintf(stderr, "%s is no V4L2 device\n",
                        dev_name.c_str());
                exit(EXIT_FAILURE);
            }
            else
            {
                errno_exit("VIDIOC_QUERYCAP");
            }
        }

        if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
        {
            fprintf(stderr, "%s is no video capture device\n",
                    dev_name.c_str());
            exit(EXIT_FAILURE);
        }

        if (!(cap.capabilities & V4L2_CAP_STREAMING))
        {
            fprintf(stderr, "%s does not support streaming i/o\n",
                    dev_name.c_str());
            exit(EXIT_FAILURE);
        }
        /* Select video input, video standard and tune here. */

        CLEAR(cropcap);

        cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

        if (0 == xioctl(fd, VIDIOC_CROPCAP, &cropcap))
        {
            crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
            crop.c = cropcap.defrect; /* reset to default */

            if (-1 == xioctl(fd, VIDIOC_S_CROP, &crop))
            {
                switch (errno)
                {
                case EINVAL:
                    /* Cropping not supported. */
                    break;
                default:
                    /* Errors ignored. */
                    break;
                }
            }
        }
        else
        {
            /* Errors ignored. */
        }

        CLEAR(fmt);

        fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        fmt.fmt.pix.width = width;
        fmt.fmt.pix.height = height;
        fmt.fmt.pix.pixelformat = pixel_format;
        fmt.fmt.pix.field = field;

        if (-1 == xioctl(fd, VIDIOC_S_FMT, &fmt))
            errno_exit("VIDIOC_S_FMT");

        /* Note VIDIOC_S_FMT may change width and height. */

        /* Buggy driver paranoia. */
        min = fmt.fmt.pix.width * 2;
        if (fmt.fmt.pix.bytesperline < min)
            fmt.fmt.pix.bytesperline = min;
        min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
        if (fmt.fmt.pix.sizeimage < min)
            fmt.fmt.pix.sizeimage = min;

        init_userp(fmt.fmt.pix.sizeimage);
    }

    void GmslCameraHandler::close_device(void)
    {
        if (-1 == close(fd))
            errno_exit("close");

        fd = -1;
    }

    void GmslCameraHandler::open_device(void)
    {
        struct stat st;

        if (-1 == stat(dev_name.c_str(), &st))
        {
            fprintf(stderr, "Cannot identify '%s': %d, %s\n",
                    dev_name.c_str(), errno, strerror(errno));
            exit(EXIT_FAILURE);
        }

        if (!S_ISCHR(st.st_mode))
        {
            fprintf(stderr, "%s is no device\n", dev_name.c_str());
            exit(EXIT_FAILURE);
        }

        fd = open(dev_name.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);

        if (-1 == fd)
        {
            fprintf(stderr, "Cannot open '%s': %d, %s\n",
                    dev_name.c_str(), errno, strerror(errno));
            exit(EXIT_FAILURE);
        }
    }

    void GmslCameraHandler::init_cuda(void)
    {
        // Number of CUDA devices
        int devCount = 0;
        cudaGetDeviceCount(&devCount);
        printf("CUDA Device Query...\n");
        printf("There are %d CUDA devices.\n", devCount);

        if (0 == devCount)
            errno_exit("No cuda device found\n");
        /* Check unified memory support. */
        cudaDeviceProp devProp;
        cudaGetDeviceProperties(&devProp, 0);
        if (!devProp.managedMemory)
        {
            printf("CUDA device does not support managed memory.\n");
            cuda_zero_copy = false;
        }

        /* Allocate output buffer. */
        size_t size = width * height * 3;
        cudaMallocManaged(&out_buffer, size, cudaMemAttachGlobal);
        cudaMallocManaged(&out_yuv_buffer, width * height * 2, cudaMemAttachGlobal);
        printf("CUDA use managed memory.");
        cudaDeviceSynchronize();
    }
} // namespace v4l_cuda
